clear,clc
pic = imread('xiaokong.jpeg');%zhangzifeng.png,,x=[100;180]; y=[125;205];
%imshow(pic);
%[x,y] = ginput(2);    %确定图像上的两点利用ginput函数，返回值是两点的坐标
x=[300;380]; y=[165;245];
pic_1 = imcrop(pic,[x(1),y(1),abs(x(1)-x(2)),abs(y(1)-y(2))]);
%利用imcrop函数对图像进行切割，输入参数是一个定点坐标，
%从该定点出发向右abs(x(1)-x(2))，向下abs(y(1)-y(2))的区域进行切割
figure(2),imshow(pic_1);
%imwrite(pic_1,'zhangzifeng_jietu.png');
pt=[x(1),y(1)]; wSize=[abs(x(1)-x(2)),abs(y(1)-y(2))];
des = drawRect(pic,pt,wSize,5 );
figure(3),imshow(des)

%自己算的pos和target_sz
pos = [abs(y(1)+y(2))/2,abs(x(1)+x(2))/2];
target_sz=[abs(y(1)-y(2)),abs(x(1)-x(2))];
%直接复制的参数，不修改
padding = 1.5;  %extra area surrounding the target目标搜索范围系数，决定了每帧进行检测的范围
lambda = 1e-4;  %regularization 正则化参数
output_sigma_factor = 0.1;  %spatial bandwidth (proportional to target)空间带宽：与目标大小成比例
show_visualization = 1;
%特征参数
interp_factor = 0.02;	
kernel.sigma = 0.5;
kernel.poly_a = 1;
kernel.poly_b = 9;
features.gray = false;
features.hog = true;
features.hog_orientations = 9;
cell_size = 4;

resize_image = (sqrt(prod(target_sz)) >= 100); 
if resize_image
    pos = floor(pos / 2);
    target_sz = floor(target_sz / 2);
end

window_sz = floor(target_sz * (1 + padding));
%pt2 = [50,75];
pt2 = [250,115];
des2 = drawRect(pic,pt2,window_sz,5 );
figure(4),imshow(des2)
output_sigma = sqrt(prod(target_sz)) * output_sigma_factor / cell_size; %用output_sigma_factor，cell_sz和跟踪框尺寸计算高斯标签的带宽output_sigma
yf = fft2(gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size))); %先用gaussian_shaped_labels生成回归标签，然后进行傅里叶变换转换到频域上的yf

%如果不移动中心到左上角
sz = floor(window_sz / cell_size);
[rs, cs] = ndgrid((1:sz(1)) - floor(sz(1)/2), (1:sz(2)) - floor(sz(2)/2)); %借助ndgrid函数生成了回归标签labels 此时生成的回归标签的峰值在中心
labels = exp(-0.5 / output_sigma^2 * (rs.^2 + cs.^2));
%yf = fft2(labels);
figure(51),mesh(rs,cs,gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size)))
figure(5),mesh(rs,cs,labels)
figure(6),mesh(rs,cs,real(yf))
cos_window = hann(size(yf,1)) * hann(size(yf,2))';

% coswin = zeros(200,200);
% for i=1:50
%     for j=1:50
%         for k=1:4
%             for l=1:4
%                 coswin((i-1)*4+k,(j-1)*4+l) = cos_window(i,j);
%             end
%         end
%     end
% end

figure(7),mesh(rs,cs,cos_window)
im = imread('xiaokong.jpeg');
if size(im,3) > 1
    im = rgb2gray(im);
end
if resize_image
    im = imresize(im, 0.5);
end
figure(8),imshow(im);
patch = get_subwindow(im, pos, window_sz);
figure(9),imshow(patch);
%fhog特征，31维
xf = fft2(get_features(patch, features, cell_size, cos_window)); %利用get_feature获得第一帧patch的特征矩阵，再经过傅里叶变换到频率域得到xf 
kf = gaussian_correlation(xf, xf, kernel.sigma);%利用gaussian_correlation得到频率域上的高斯响应kf
alphaf = yf ./ (kf + lambda);   %equation for fast training岭回归计算，得到分类器参数alphaf
model_alphaf = alphaf;
model_xf = xf;

figure(10),mesh(rs,cs,real(kf))
figure(11),mesh(rs,cs,real(alphaf))

pos=pos+10;
frame = 2;
im = imread('xiaokong.jpeg');
if size(im,3) > 1,
    im = rgb2gray(im);
end
patch = get_subwindow(im, pos, window_sz);%在上一帧的跟踪结果pos的基础上，根据window_sz在这一帧图像上提取检测区域patch 这一帧的训练，与下一帧的检测，使用的是同一块patch 
zf = fft2(get_features(patch, features, cell_size, cos_window));%利用get_feature得到检测区域patch的特征zf 
kzf = gaussian_correlation(zf, model_xf, kernel.sigma);%由zf和model_xf计算高斯响应kzf 
response = real(ifft2(model_alphaf .* kzf));  %equation for fast detection model_alphaf与kzf点乘后进行傅里叶反变换，回到时域。保留实部，得到实数响应图response map，并且响应均为归一化值

figure(12),mesh(rs,cs,response)
[vert_delta, horiz_delta] = find(response == max(response(:)), 1);
if vert_delta > size(zf,1) / 2,  %wrap around to negative half-space of vertical axis
    vert_delta = vert_delta - size(zf,1);
end
if horiz_delta > size(zf,2) / 2,  %same for horizontal axis
    horiz_delta = horiz_delta - size(zf,2);
end
pos2 = pos + cell_size * [vert_delta - 1, horiz_delta - 1];

%%给图像画网格
img = patch;
figure(13);
subplot(1,2,1)
imshow(img);
title('原始图像')

M=51;
N=51;
%subplot(1, 2, 2); 
figure(132),imshow(img);
% title('网格标记图像', 'FontWeight', 'Bold');
hold on;
[xt, yt] = meshgrid(round(linspace(1, size(img, 1)+1, M)),round(linspace(1, size(img, 2)+1, N)));%生成数据点矩阵
xt=xt-0.5;yt=yt-0.5;
mesh(yt, xt, zeros(size(xt)), 'FaceColor','None', 'LineWidth', 1,'EdgeColor', 'r');%绘制三维网格图


img = patch;
[feature,vision] = extractHOGFeatures(double(img),'CellSize',[4 4]);
%figure(14);
imshow(img);
hold on
plot(vision)

img = patch;
[Gmag,Gdir] = imgradient(img,'intermediate');

% Emag = [18.0277563773199,41.6773319683494,21.5870331449229,44.7213595499958;
%         39.8497176903426,16.4012194668567,41.6773319683494,45.4862616621766;
%         45.7930125674212,25.2982212813470,50,36.2353418639869;
%         14.7648230602334,50.9901951359279,47.8016736108685,19.8494332412792];
% Edir = [176.820169880136,149.743562836471,-13.3924977537511,-26.5650511770780;
%         162.474431626277,127.568592028828,-30.2564371635293,-33.3407073464770;
%         148.392497753751,-18.4349488229220,-36.8698976458440,-50.5993393365206;
%         118.300755766006,-25.5599651718238,-37.3493490446406,-49.0856167799749];
Emag =[61.4003257320350,53.6003731330296,5.09901951359278,20.6155281280883;
       63.8122245341753,20.8806130178211,13.4536240470737,13.9283882771841;
       46.6476151587624,8.94427190999916,5.65685424949238,7.21110255092798;
       24.0208242989286,12.8062484748657,5,7.61577310586391];
Edir = [-30.3236068625500,-36.6561084159669,101.309932474020,-165.963756532074;
        -32.1957339347133,-73.3007557660064,-48.0127875041833,-158.962488974578;
        -30.9637565320735,-116.565051177078,-45,146.309932474020;
        -2.38594403038881,128.659808254090,143.130102354156,66.8014094863518];
bin = zeros(1,9);
for i=1:16
    if Edir(i)<0
        Edir(i) = Edir(i)+180;
        shang = floor(Edir(i)/20)+1;
        yu    = mod(Edir(i),20);
        zuo = yu/20*Emag(i);
        you = Emag(i)-zuo;
        bin(shang)=bin(shang)+zuo;
        if shang<9
            bin(shang+1)=bin(shang+1)+you;
        else
            bin(1)=bin(1)+you;
        end
    end
end
figure(15)
bar(bin)
